Distributed Navigation with Unknown Initial Poses and Data Association via Expectation Maximization

We present a novel approach for multi-robot distributed and incremental inference over variables of interest, such as robot trajectories, considering the initial relative poses between the robots and multi-robot data association are both unknown. Assuming robots share with each other informative observations, this inference problem is formulated within an Expectation-Maximization (EM) optimization, performed by each robot separately, alternating between inference over variables of interest and multi-robot data association. To facilitate this process, a common reference frame between the robots should first be established. We show the latter is coupled with determining multi-robot data association, and therefore concurrently infer both using a separate EM optimization. This optimization is performed by each robot starting from several promising initial solutions, converging to locally-optimal hypotheses regarding data association and reference frame transformation. Choosing the best hypothesis in an incremental problem setting is in particular challenging due to high sensitivity to measurement aliasing and possibly insufficient amount of data. Selecting an incorrect hypothesis introduces outliers and can lead to catastrophic results. To address these challenges we develop a model-selection based approach to choose the most probable hypothesis, while resorting to Chinese Restaurant Process to represent statistical knowledge regarding hypothesis prior probabilities. We evaluate our approach in real-data experiments.